typedef struct {
    double Kp, Ki, Kd;
    double integral, prev_error;
    double output_min, output_max; // 抗饱和限幅
} PIDController;

double PID_Compute(PIDController *pid, double setpoint, double input, double dt) {
    double error = setpoint - input;
    pid->integral += error * dt; // 积分累积
    double derivative = (error - pid->prev_error) / dt; // 微分项
    double output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
    // 限幅处理
    output = (output > pid->output_max) ? pid->output_max : (output < pid->output_min) ? pid->output_min : output;
    pid->prev_error = error;
    return output;
}